package robotRouting;

import java.util.ArrayList;
import monitor.Node;

public class AngleFilter {
	private int[] angles;
	private int count;	
	private int errorPermission;
	private int angle_average=0;
	
	public AngleFilter(int[] angles, int count, int errorPermission) {
		super();
		this.angles = angles;
		this.count = count;
		this.errorPermission = errorPermission;
	}

	
	// do not use channel in this version
	// int channel;

	private void angleAverage() {
		if (count == 0)
		{angle_average=0;
			return;}

        int total_angle=0;
		for (int i = 0; i < count; i++) {
			total_angle += angles[i];
		}
		angle_average= total_angle / count;
	}

	
	public void filter(){
		if (count == 0)
		  return;

		angleAverage();
		ArrayList<Integer> angleList= new ArrayList<Integer>();
		for(int i=0;i<count;i++){
		if(Math.abs(angles[i]-angle_average)<= errorPermission)
		{
			angleList.add(angles[i]);
		}						
		}
		
		count=angleList.size();
		
		for(int i=0;i<count;i++)
		{
			angles[i]= angleList.get(i);
			
		}
		
	}
	
	public int angleAfterFilter(){
		
		angleAverage();
		System.out.println("Average Angle after adjustment is:"+angle_average);
		return angle_average;
		
	}
	
    public void angleDisply(){
    
    	for (int i = 0; i < count; i++) {
			System.out.print(angles[i]+" ");
		}
    	System.out.println();
    }	
	
	public static void main(String args[]){
		int angles []={1,2,3,4,5,6,7,8,9};
		
		AngleFilter filter = new AngleFilter(angles,angles.length,1);
		
		System.out.println("Before filtering:");
		filter.angleDisply();
		
		System.out.println("After filtering:");
		filter.filter();
		filter.angleDisply();
		filter.angleAfterFilter();
//		System.out.println(filter.angleAfterFilter());
		
	}
	
}
